
#include <lipid.hpp>

#include "io_expansion.hpp"
#include "app_config.hpp"

// ======================= PCF8574 ======================

// PCF8574
PCF8574 ex_port1{PCF8574_ADDR_1};
PCF8574 ex_port2{PCF8574_ADDR_2};


/**
 * @brief 将两片PCF8574 都初始化为高电平
 *
 */
void init_pcf8574() {
    ex_port1.begin(0xff);
    ex_port2.begin(0xff);
}

static u8 _ex1_cache = 0xff;


u8 ex1_cache() {
    return _ex1_cache;
}


void write_ex1(u8 b) {
    b |= EX_1_INPUT_MASK;
    _ex1_cache = b;
    ex_port1.write8(b);
}

u8 read_ex1() {
    u8 b = ex_port1.read8();
    _ex1_cache = b;
    return b;
}


void set_ex1(u8 set_mask, u8 clear_mask) {
    u8 b = _ex1_cache;
    b &= clear_mask;
    b |= set_mask;
    write_ex1(b);
}


bool test_ex1(u8 mask) {
    u8 b = _ex1_cache;
    b &= mask;
    return (b == mask);
}


// =======================  ADC  ========================

static ad_channel current_ad_channel = ad_channel::battery;  // 上电后默认通道是电池


void set_ad_channel(ad_channel as) {
    current_ad_channel = as;
    u8 as_bit = static_cast<u8>(as);
    u8 as_mask = ~(static_cast<u8>(_BV(EX_AS0) | _BV(EX_AS1) | _BV(EX_AS2)));
    set_ex1(as_bit, as_mask);
}


static float ad_cache;


float read_voltage() {
    ad_cache = analogRead(A0) * AD_FACTOR;
    return ad_cache;
}


float read_ad_cache() {
    return ad_cache;
}


/**
 * @brief 快速定时切换0 和1 通道，用示波器测量切换后的稳定时间
 *
 * 示波器一个通道测量AS0 引脚，低电平表示0 通道，高电平为1 通道；另一个通道测量ADC 输入。
 * 通道0 和1 接不同的电压，比如0V 和5V，测量CD4051 的切换时间和ADC 输入的上升时间。
 *
 * 主程序可以调用read_ad_cache 获取最近的测量值。
 *
 * @return TimeType
 */
TimeType task_test_ad_channel_switch() {
    static bool is_as0 = false;
    if (is_as0) {
        is_as0 = false;
        set_ad_channel(ad_channel::_0);
    }
    else {
        is_as0 = true;
        set_ad_channel(ad_channel::_1);
    }
    // 可以根据实测结果调整延时数值
    delayMicroseconds(10);
    // 切换通道后的短延时不太适合用定时回调实现，
    // 因为其他任务可能也要切换通道，再加一个锁就有点麻烦
    read_voltage();
    return 10;  // 10 毫秒周期
}


// 窗口大小8 的滑动平均滤波器，处理浮点数据
using FilterType = lipid::MovingAverageFilter<float, float, BATTERY_MEASURE_WINDOW_SIZE>;

static FilterType battery_filter{};


void init_battery_measure() {
    // 先一次，用这个值把滤波器窗口填满
    set_ad_channel(ad_channel::battery);
    float v = read_voltage();
    battery_filter.reset(v);
}


TimeType task_battery_measure() {
    ad_channel prev = ad_channel::battery;
    if (current_ad_channel != ad_channel::battery) {
        prev = current_ad_channel;
        set_ad_channel(ad_channel::battery);
        delayMicroseconds(AD_SWITCH_DELAY_US);
        battery_filter.feed(read_voltage());
        set_ad_channel(prev);
        delayMicroseconds(1);
    }
    else {
        battery_filter.feed(read_voltage());
    }
    return BATTERY_MEASURE_PEROID;
}


float battery_voltage() {
    return battery_filter.get_average();
}


